Copyright □ 1999, by Professor Jong-Hwan Kim, Korea Advanced Institute of Science and Technology (KAIST).  All rights reserved. Reproduction or translation of any part of this work is unlawful. Request for permission or further information should be addressed to Professor Jong-Hwan Kim at <johkim@vivaldi.kaist.ac.kr> or <Tel: +82-42-869-3448>.

Prof. Jong-Hwan Kim	Intelligent Control Lab, KAIST

9.로봇 제어 프로그램 Ⅲ

임의의 위치에 있는 로봇이 공을 목표점으로 차게 하는 함수인 Shoot 에 대해서 알아보자.

9.1 Shoot 함수

Posture 함수는 로봇을 원하는 위치로 움직이게 하는 함수로 목표점에서 로봇의 방향각은 고려되지 않았다. 즉, 로봇을 목표위치[eqn: rm (t_x , t_y )]로 이동하게 하는데 로봇이 목표위치에 도달할 때의 각도는 고려되지 않았다. 그러므로 이 상태에서 로봇이 공을 찰 경우 대부분의 경우 방향각이 틀리므로 공을 원하는 목표지점으로 보내지 못한다.

그림 9. 로봇의 경로들

그림 9.1에서 살펴보면 로봇이 목표점 1 (Target 1)으로 공을 차기 위해서는 경로 1 (Path 1)을 따라 이동해야 하고 목표점 2 (Target 2)로 공을 차기 위해서는 경로 2 (Path 2)를 따라 움직여야 한다. 이때 로봇이 공의 위치에 도달할 때의 각도는 각각[eqn: -theta_1 , theta_2]이다. 결국 그림 9.1에서 알 수 있듯이 로봇이 공을 원하는 목표점으로 차기 위해서는 로봇이 공의 위치로 도달할 때의 각도도 고려해야 한다.

로봇이 공을 목표점으로 차게 하는 함수가 Shoot 이다. Shoot 함수에서는 로봇이 공을 원하는 목표점으로 보내기 위한 각도를 계산하고 이에 따라 로봇의 왼쪽, 오른쪽 바퀴의 속도 입력을 계산한다.

그림 9.3 Shoot 함수에서 로봇의 각도계산

그림 9.2에서 공의 위치를 Target 1으로 하고, 목표점(Goal)을 Target 2로 하자. 로봇이 Target 1의 공을 차서 목표점인 Target 2로 보내기 위해서는 로봇의 방향이 [eqn: phi_3]의 각도로 향하고 있어야 한다. 매순간 로봇이 향해야하는 각도는 [eqn: phi_3]로 이는 다음과 같이 계산된다. 여기서 X-Y 좌표계에서 로봇의 현재위치는[eqn: ( r_x , r_y )], 공의 위치는 Target 1으로[eqn: ( b_x , b_y )], 목표점은 Target 2는[eqn: (t_x, t_y )]이다.

[eqn: &phi_3 = 2phi_2 - phi_1, # &phi_1 = tan^{-1} ( dy_1 over dx_1 ), # &phi_2 = tan^{-1} ( dy_2 over dx_2 )]

(9.1)

여기서[eqn: &dx_1 = t_x - b_x , dy_1 = t_y - b_y , &dx_2 = r_x - b_x , dy_2 = r_y - b_y]이다. 이 관계식은 그림 9.2에서 쉽게 확인할 수 있다.

로봇이 현재 위치에서 공을 지면의 오른쪽 방향으로 차기 위하여 따라나가야 할 방향각[eqn: phi_3]를 식 (9.1)을 이용하여 컴퓨터 시뮬레이션으로 구해보면 그림 9.3과 같다.

그림 9.3 Shoot 함수의 시뮬레이션 결과

그림 9.3에서 작은 원은 로봇이 위치한 지점을 나타내고 원에 붙은 작은 선은 로봇이 위치한 곳에서 로봇이 향해야 하는 방향각을 나타낸다.

그림 9.3과 같이 경기장내 임의의 위치에 있는 로봇이 공을 차기 위한 방향각 궤적을 구하였다. 다음은 이러한 방향각을 따라가게 하기 위한 양쪽바퀴 속도제어 문제를 다룬다. 이를 위하여 그림 9.2에서의 관계를 실제로 경기장 위의 로봇과 공으로 그림 9.4에 나타내었다. 그림 9.4에서는 로봇이 [eqn: (r_x , r_y )]에 위치했을 때[eqn: (b_x , b_y )]에 위치한 공을 상대편 골대[eqn: (t_x , t_y )]로 차기 위해서는[eqn: (r_x , r_y )]의 위치에서 [eqn: phi_3]의 각도로 접근해야 한다. 즉, 로봇이 향해야 하는 방향각이 [eqn: phi_3]가 된다. 이때 로봇의 방향각 [eqn: theta_r]과의 각도오차 [eqn: theta_e]는 다음과 같다:

[eqn: &theta_e ~= phi_3 ~-~theta_r]

(9.2)

여기서 [eqn: theta_r]은 로봇의 현재 위치에서의 방향각이다. 로봇중심 속도[eqn: v_c]를 고정했을 경우 오른쪽 바퀴와 왼쪽 바퀴의 속도 입력은 다음과 같이 구해진다.

[eqn: & v_R = v_c + K_\angle CDOT theta_e# & v_L = v_c - K_\angle CDOT theta_e]

(9.3)

여기서 [eqn: K_\angle]은 각도오차에 대한 비례상수로 제어기 변수이다.

그림 9. 경기자의 로봇과 공에서 구한 각도

이상을 프로그램으로 나타낸 것이 다음의 Shoot 함수이다. 함수의 인자는 어떤 로봇을 지정하는 whichrobot과 공의 위치를 지정하는[eqn: b_x , b_y]와 목적위치를 지정하는[eqn: t_x , t_y]이다.

void Shoot(int whichrobot, double bx, double by, double tx, double ty)

{

double dx1, dy1, dx2, dy2, Vc = 50.0, Ka = 0.22	// 변수 선언

int a1, a2, a3, vl, vr, theta_e;

dx1 = tx - bx;	// dx1 계산

dy1 = ty - by;	// dy1 계산

if(dx1 == 0 && dy1 == 0)	a1 = 90;	// [eqn: phi_1] 계산

else	a1 = (int)(180/M_PI*atan2((double)(dy1), (double)(dx1)));

// M_PI = [eqn: pi]

dx2 = bx - PositionOfHomeRobot[whichrobot][0];	// dx2 계산

dy2 = by - PositionOfHomeRobot[whichrobot][1];	// dy2 계산

if(dx2==0 && dy2==0)	a2 = 90;	// [eqn: phi_2] 계산

else	a2 = (int)(180/M_PI*atan2((double)(dy2),(double)(dx2)));

a3 = 2*a2 - a1;	// [eqn: phi_3] 계산

theta_e = a3-AngleOfHomeRobot[whichrobot];	// [eqn: theta_e]=[eqn: phi_3 - theta_r] 계산

while(theta_e > 180)	theta_e -= 360;	// 각도를 [eqn: -180Deg sim 180Deg]이내로 계산

while(theta_e < -180)	theta_e += 360;

if(theta_e < -90){	// [eqn: theta_e]<[eqn: -90Deg]일 때 로봇의 방향을 바꿈

theta_e += 180;

Vc = -Vc;

}

else if(theta_e > 90){	// [eqn: theta_e]>[eqn: 90Deg]일 때 로봇의 방향을 바꿈

theta_e -= 180;

Vc = -Vc;

}

if(abs(theta_e) > 50)	// 각도 오차에 따라 제어 계수 변화시킴

Ka = 0.16;	// 각도 오차가 작아짐에 따라 제어 계수의

else if(abs(theta_e > 40))	크기를 증가시킴

Ka = 0.18;

else if(abs(theta_e > 30))

Ka = 0.2;

else if(abs(theta_e > 20))

Ka = 0.22;

else

Ka = 0.24;

vl = (int)(Vc - Ka*(double)theta_e);	// 왼쪽 바퀴의 제어입력 속도

vr = (int)(Vc + Ka*(double)theta_e);	// 오른쪽 바퀴의 제어입력 속도

Velocity(whichrobot,vl,vr);

}

그림 9.4에서도 알 수 있듯이 공이 놓여있는 직선 상에서는 로봇이 향해야 하는 방향각이 심하게 변한다. 즉, 로봇이 공을 차기 위해서 공보다 위쪽에서 접근할 때는 로봇이 향해야 하는 각은 음(-)의 각이다. 그러다가 로봇이 공보다 아래쪽에 위치하면 로봇이 향해야 하는 각은 양(+)의 각이 된다. 결국, 로봇이 먼 곳에서 공을 차기 위해 접근해오다가 공과 같은 선상에 위치하여 접근해 올 때는 의 아래로 흔들림 현상이 생긴다. 이를 그림 9.5에서 나타내었다.

그림 9. 로봇의 흔들림 현상

그리고 로봇이 공보다 오른쪽에 위치했을 때 공을 차기 위해서는 커다란 호를 그리면서 공의 뒤쪽으로 움직여야 한다. 결국, 로봇이 공보다 오른쪽에 있을 때 Shoot 함수를 수행하면 필요이상의 커다란 호 궤적을 그리면서 움직이므로 단점이 된다.

그림 9.6에 로봇의 흔들림 현상을 해결하기 위한 개선된 방향각을 나타내었다. 수정된 영역에서 로봇이 향해야 하는 방향각은 공 쪽으로 향한다. 이렇게 함으로써 로봇이 공 근처에서 흔들림 현상을 방지할 수 있다. 이에 대하여는 다음절에 다시 살펴보기로 한다.

그림 9. 개선된 Shoot 시뮬레이션 결과